/*
 * Gyro.cpp
 *
 *  Created on: Jan 14, 2014
 *      Author: ppks
 */

#include "common.h"
#include "IMU_Common.h"
#include "i2c.h"
#include "FlightController_Common.h"
#include "EEPROM.h"
#include "Alarms.h"

/******************************************************************************/
/* Macro Definition                                                           */
/******************************************************************************/
#if !defined(RECORD_GYRO_ORIENTATION_DATA)
  #define RECORD_GYRO_ORIENTATION_DATA(X, Y, Z) {imuData.gyroADC[ROLL] = Y; imuData.gyroADC[PITCH] = -X; imuData.gyroADC[YAW] = -Z;}
#endif

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
static void Gyroscope_Common(void);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name: Gyro_Init()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Gyroscope_Init(void)
{
#if (GYROSCOPE_TYPE == MPU6050)
    CommI2C_WriteReg(GYRO_ADDRESS, 0x6B, 0x80);              /* PWR_MGMT_1    -- DEVICE_RESET 1 */

    /* Wait 5ms to allow the device reset */
    delay(5);

    CommI2C_WriteReg(GYRO_ADDRESS, 0x6B, 0x03);              /* PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) */
    CommI2C_WriteReg(GYRO_ADDRESS, 0x1A, MPU6050_DLPF_CFG);  /* CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz) */
    CommI2C_WriteReg(GYRO_ADDRESS, 0x1B, 0x18);              /* GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec */

    /* enable I2C bypass for AUX I2C */
#if MAG_INSTALLED
    CommI2C_WriteReg(GYRO_ADDRESS, 0x37, 0x02);              /* INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=1 ; CLKOUT_EN=0 */
#endif /* MAG_INSTALLED */
#endif /* (GYROSCOPE_TYPE == MPU6050) */
}

/*******************************************************************************
* Function Name: Accelerometer_ReadRawADC()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Gyroscope_ReadRawADC(void)
{
    uint8_t rawADC[6];

    /* Read the data from accelerometer */
#if (GYROSCOPE_TYPE == MPU6050)
    CommI2C_ReadRegToBuffer(GYRO_ADDRESS, GYRO_DATA_REGISTER, rawADC, 6);

    /* Record the data into the global structure */
    RECORD_GYRO_ORIENTATION_DATA( ((rawADC[0]<<8) | rawADC[1])>>3 ,   /* X */
                                  ((rawADC[2]<<8) | rawADC[3])>>3 ,   /* Y */
                                  ((rawADC[4]<<8) | rawADC[5])>>3 );  /* Z */
#endif /* (GYROSCOPE_TYPE == MPU6050) */

    /* Call common code that applies baselines or performs calibration */
    Gyroscope_Common();
}

/*******************************************************************************
* Function Name: Gyroscope_Common()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void Gyroscope_Common(void)
{
    static int16_t previousGyroADC[3] = {0,0,0};
    static int32_t g[3];
    uint8_t axis;
#if defined(GYROCALIBRATIONFAILSAFE)
    uint8_t tilt = 0;
#endif /* #if defined(GYROCALIBRATIONFAILSAFE)*/

    if (calibration.GyroCycles > 0)
    {
        for (axis = 0; axis < 3; axis++)
        {
            /* Reset g[axis] at start of calibration */
            if (calibration.GyroCycles == GYRO_CALIBRATION_CYCLES_DEFAULT)
            {
                g[axis] = 0;

                /* Initialize baselines to zero */
                calData.gyroBaseline[axis] = 0;

        #if defined(GYROCALIBRATIONFAILSAFE)
                previousGyroADC[axis] = imuData.gyroADC[axis];
            }
            if (calibratingG % 10 == 0)
            {
                if(abs(imuData.gyroADC[axis] - previousGyroADC[axis]) > 8)
                {
                    tilt = 1;
                }
                previousGyroADC[axis] = imuData.gyroADC[axis];
        #endif
            }

            /* Sum up 512 readings */
            g[axis] += imuData.gyroADC[axis];

            /* Clear the ADC reading */
            imuData.gyroADC[axis] = 0;

            /* Check if it is the last cycle and update baselines */
            if (calibration.GyroCycles == 1)
            {
                calData.gyroBaseline[axis] = (g[axis] + 256) >> 9;
            }
        }

        /* Check if it is the last calibration cycle */
        if (calibration.GyroCycles == 1)
        {
        #if defined(BUZZER)
            Alarms_SetAlarmLevel(ALARM_EVENT_CONFIRMATION, ALARM_LEVEL_4);
        #else /* defined(BUZZER) */
            Alarms_BlinkLED(10, 15, 1); /* the delay causes to beep the buzzer really long */
        #endif /* defined(BUZZER) */
            /* Set the flag that calibration is completed */
            SetGyroCalibratedFlag();
#if UPDATE_CAL_DATA_AFTER_ANY_SENSOR
            /* Update calibration data in EEPROM */
            EEPROM_UpdateCalibrationData();
#endif /* UPDATE_CAL_DATA_AFTER_ANY_SENSOR */
        }

    #if defined(GYROCALIBRATIONFAILSAFE)
        if (tilt)
        {
            calibration.GyroCycles = 1000;
            LEDPIN_ON;
        }
        else
        {
            calibration.GyroCycles--;
            LEDPIN_OFF;
        }
        return;
    #else
        calibration.GyroCycles--;
    #endif
    }

    /* If not in the calibration loop - just subtract the calibration data */
    for (axis = 0; axis < 3; axis++)
    {
        imuData.gyroADC[axis]  -= calData.gyroBaseline[axis];
        /* anti gyro glitch, limit the variation between two consecutive readings */
        imuData.gyroADC[axis] = constrain(imuData.gyroADC[axis], previousGyroADC[axis] - GYRO_GLITCH_RANGE, previousGyroADC[axis] + GYRO_GLITCH_RANGE);

        previousGyroADC[axis] = imuData.gyroADC[axis];
    }

#if defined(SENSORS_TILT_45DEG_LEFT)
    int16_t temp  = ((imuData.gyroADC[PITCH] - imuData.gyroADC[ROLL] )*7)/10;
    imuData.gyroADC[ROLL] = ((imuData.gyroADC[ROLL]  + imuData.gyroADC[PITCH])*7)/10;
    imuData.gyroADC[PITCH]= temp;
#endif
#if defined(SENSORS_TILT_45DEG_RIGHT)
    int16_t temp  = ((imuData.gyroADC[PITCH] + imuData.gyroADC[ROLL] )*7)/10;
    imuData.gyroADC[ROLL] = ((imuData.gyroADC[ROLL]  - imuData.gyroADC[PITCH])*7)/10;
    imuData.gyroADC[PITCH]= temp;
#endif
}



